#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
    发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
    准备：
        话题：/turtle1/pose
        消息：/turtlesim/Pose

    话题查找：rostopic list
        /rosout
        /rosout_agg
        /turtle1/cmd_vel
        /turtle1/color_sensor
        /turtle1/pose
    查看：rostopic info /turtle1/pose
        Type: turtlesim/Pose
    查看：rosmsg info turtlesim/Pose
        float32 x
        float32 y
        float32 theta
        float32 linear_velocity
        float32 angular_velocity

    流程：
        1.包含文件 
        2.设置编码、初始化、NodeHandle 
        3.创建订阅对象，订阅 /turtle1/pose
        4。回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布 （关注）
        5. spin()
*/

void doPose(const turtlesim::Pose::ConstPtr& pose) 
{
    // 获取位姿信息，转换成坐标系相对关系（核心），并发布
    // a。 创建发布对象；这里设置 为静态，就不需要每次都创建
    static tf2_ros::TransformBroadcaster pub;

    // b。组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    // 坐标系偏移设置 
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    // 坐标系四元数
    /*
        位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是2D ，没有翻滚与俯仰角度，所以可以 得出乌龟
        的欧拉角：0 0 theta
    */
    tf2::Quaternion qtn;
    qtn.setRPY(0, 0, pose->theta);

    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();
    
    // c。发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    // 2.设置编码、初始化、NodeHandle 
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "dynamic_pub");
    ros::NodeHandle nh;

    // 3.创建订阅对象，订阅 /turtle1/pose
    ros::Subscriber sub = nh.subscribe("/turtle1/pose", 100, doPose);
    // 4。回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布 （关注）
    // 5. spin()
    ros::spin();


    return 0;
}
